/*
 *  consdis.h
 *  EPPI-0.0
 *
 *  Created by Juan Carlos Garcia Orden on 26/08/2011
 *
 */

#ifndef _consdis_h_
#define _consdis_h_

#include "constraint.h"
#include "commonutil.h"


using namespace std;

class Consdis : public Constraint
{
public:
	double initialDis;
	VectorXd r;
	MatrixXd A;
	VectorXd dr;
	MatrixXd ddr;

public:
	Consdis();
	Consdis(vector<string>&, int);
	Consdis(vector<string>&, int, double);
	~Consdis(){}

	// Virtual functions from base
	/*
	void calcPhi(const vector<double>&);
	void calcDPhi(const vector<double>&);
	void calcDDPhi(const vector<double>&);
	void setFqSymmetries();
	void setKqSymmetries();
	 */
	
	void calcPhi(const VectorXd &);
	void calcDPhi(const VectorXd &);
	void calcDDPhi(const VectorXd &);
	void setFqSymmetries();
	void setKqSymmetries();
	double getScalar(const VectorXd &);
	VectorXd getDScalar(const VectorXd &);
	MatrixXd getDDScalar(const VectorXd &);
	void setParamAtCons(vector<double> &);
	
	/** Local functions */

	/** Friend functions */
	friend double lTwoNorm(const vector<double>& v1);
	friend double lTwoNorm(const vector<double>& v1, const vector<double>& v2);
	friend void uVersor(const vector<double>& v1, const double & dis, const int& dim, vector<double>& u);
//	friend void uVersor(const vector<double>& v1, const vector<double>& v2, const double & dis, vector<double>& u);
	friend void printSTLVector(vector<double>& v);
	friend void printSTLMatrix(const vector< vector<double> >& m);


};

#endif

